#include "stdafx.h"
#include "Locate.h"
#include "MFC_ControlDlg.h"

using namespace HalconCpp;
Locate1::Locate1()
{
	isModelLoaded = false;
}

int Locate1::loadModel()
{
	// Local control variables
	if (!isModelLoaded)
	{
		HalconCpp::HTuple hv_ErrorVar;
		try
		{
			hv_ErrorVar = 2;
			HalconCpp::ReadDeformableModel("normal-1.dfm", &hv_ModelID);
		}
		catch (HalconCpp::HException e)
		{
			hv_ErrorVar = (int)e.ErrorNumber();
			if (hv_ErrorVar < 0)
				throw e;
			return -1;
		}
	}
	return 1;
}

int Locate1::locate(Pose &poseLocate)
{
	// Local iconic variables
	HalconCpp::HObject  ho_Image;

	HalconCpp::HTuple  hv_CovPose, hv_Score;

	HalconCpp::ReadImage(&ho_Image, "cache.png");
	try{
		HalconCpp::FindPlanarCalibDeformableModel(ho_Image, hv_ModelID,   //线性变形模板匹配，模板在行列方向上可以进行适当的缩放
			-0.39, 6.29,//Start and extand angle
			1, 1, //Row scale min and max
			1, 1, //Column scale min and max
			0.8,  //Min score
			1,    //Number matches
			1,    //Max overlap
			0,    //Number levels
			0.1,  //Greediness
			HalconCpp::HTuple(),
			HalconCpp::HTuple(),
			&hv_Pose, &hv_CovPose, &hv_Score);
	}
	catch (HalconCpp::HException &except)
	{
		except.ErrorNumber();
	}
	HalconCpp::ClearShapeModel(hv_ModelID);
	if (hv_Pose.Length() != 0)
	{
		poseLocate = genPose((double)hv_Pose[0] * 1000, (double)hv_Pose[1] * 1000,
			(double)hv_Pose[2] * 1000, (double)hv_Pose[3], (double)hv_Pose[4],
			(double)hv_Pose[5]);
		poseLocate.rx = poseLocate.rx>180 ? poseLocate.rx - 360 : poseLocate.rx;
		poseLocate.ry = poseLocate.ry>180 ? poseLocate.ry - 360 : poseLocate.ry;
		poseLocate.rz = poseLocate.rz>180 ? poseLocate.rz - 360 : poseLocate.rz;
		//保存定位结果
		saveResult();

		return 1;
	}
	else
		return -1;
}

void Locate1::saveResult()
{
	// Local iconic variables
	HalconCpp::HObject  ho_Image, ho_ModelContours, ho_FoundContour;
	HalconCpp::HObject  ho_ObjectSelected, ho_ModelWorld;

	// Local control variables
	HalconCpp::HTuple  hv_CamParam;
	HalconCpp::HTuple  hv_WindowDrawResult;
	HalconCpp::HTuple  hv_HomMat3D, hv_NumberContour;
	HalconCpp::HTuple  hv_K, hv_Y, hv_X, hv_Z, hv_Xc, hv_Yc, hv_Zc, hv_R;
	HalconCpp::HTuple  hv_C;

	hv_CamParam.Clear();
	hv_CamParam[0] = 0.00840228644304546;
	hv_CamParam[1] = -1019.55707294893;
	hv_CamParam[2] = 4.40722435287872e-006;
	hv_CamParam[3] = 4.4e-006;
	hv_CamParam[4] = 828.65339497083;
	hv_CamParam[5] = 633.46017521628;
	hv_CamParam[6] = 1624;
	hv_CamParam[7] = 1234;

	HalconCpp::ReadImage(&ho_Image, "cache.png");
	//获取轮廓投影
	HalconCpp::GetDeformableModelContours(&ho_ModelContours, hv_ModelID, 1);
	HalconCpp::GenEmptyObj(&ho_FoundContour);
	HalconCpp::CountObj(ho_ModelContours, &hv_NumberContour);
	HalconCpp::HTuple end_val40 = hv_NumberContour;
	HalconCpp::HTuple step_val40 = 1;
	for (hv_K = 1; hv_K.Continue(end_val40, step_val40); hv_K += step_val40)
	{
		HalconCpp::SelectObj(ho_ModelContours, &ho_ObjectSelected, hv_K);
		HalconCpp::GetContourXld(ho_ObjectSelected, &hv_Y, &hv_X);
		hv_Z = HalconCpp::HTuple(hv_X.TupleLength(), 0.0);
		//Transform the metric model into world coordinates.
		HalconCpp::PoseToHomMat3d(hv_Pose, &hv_HomMat3D);
		HalconCpp::AffineTransPoint3d(hv_HomMat3D, hv_X, hv_Y, hv_Z, &hv_Xc, &hv_Yc, &hv_Zc);
		//Project the 3D points of the model, which are
		//available as world coordinates, into the image
		//coordinate system.
		HalconCpp::Project3dPoint(hv_Xc, hv_Yc, hv_Zc, hv_CamParam, &hv_R, &hv_C);
		HalconCpp::GenContourPolygonXld(&ho_ModelWorld, hv_R, hv_C);
		HalconCpp::ConcatObj(ho_FoundContour, ho_ModelWorld, &ho_FoundContour);
	}
	HalconCpp::OpenWindow(0, 0, 1624, 1234, 0, "invisible",
		"", &hv_WindowDrawResult);
	HalconCpp::DispImage(ho_Image, hv_WindowDrawResult);
	HalconCpp::SetColor(hv_WindowDrawResult, "magenta");
	HalconCpp::SetLineWidth(hv_WindowDrawResult, 5);
	HalconCpp::DispXld(ho_FoundContour, hv_WindowDrawResult);
	HalconCpp::DumpWindow(hv_WindowDrawResult, "png", "ResultSaved1");
	HalconCpp::CloseWindow(hv_WindowDrawResult);
}


Locate2::Locate2()
{
	isModelLoaded = false;
}

int Locate2::loadModel()
{
	// Local control variables
	if (!isModelLoaded)
	{
		HalconCpp::HTuple hv_ErrorVar, hv_ModelFile;
		try
		{
			hv_ErrorVar = 2;
			HalconCpp::ReadDeformableModel("piston-holes-template-04.dfm", &hv_ModelID);
			//            hv_ModelFile = "model_nut.sbm";
			//            HalconCpp::ReadShapeModel(hv_ModelFile, &hv_ReusedModelID);
		}
		catch (HalconCpp::HException e)
		{
			hv_ErrorVar = (int)e.ErrorNumber();
			if (hv_ErrorVar < 0)
				throw e;
			return -1;
		}
	}
	return 1;
}

int Locate2::locate(Pose &poseLocate)
{
	// Local iconic variables
	HalconCpp::HObject  ho_ReusedShapeModel, ho_Cross, ho_Image, ho_FoundContour;
	HalconCpp::HObject  ho_ModelAtNewPosition;


	// Local control variables
	HalconCpp::HTuple  hv_WindowHandle, hv_ModelFile, hv_ReusedModelID;
	HalconCpp::HTuple  hv_ReusedRefPointRow, hv_ReusedRefPointCol, hv_NumLevels;
	HalconCpp::HTuple  hv_AngleStart, hv_AngleExtent, hv_AngleStep, hv_ScaleMin;
	HalconCpp::HTuple  hv_ScaleMax, hv_ScaleStep, hv_Metric, hv_MinContrast;
	HalconCpp::HTuple  hv_ObjectRow, hv_ObjectCol, hv_WorldX, hv_WorldY;
	HalconCpp::HTuple  hv_CamParam, hv_WorldZ, hv_PoseModelPlane, hv_Quality;
	HalconCpp::HTuple  hv_RowCheck, hv_ColumnCheck, hv_AngleCheck, hv_ScaleCheck;
	HalconCpp::HTuple  hv_Score, hv_i, hv_MovementOfObject, hv_MoveAndScalingOfObject;
	HalconCpp::HTuple  hv_Row, hv_Column, hv_X, hv_Y;

	//
	HalconCpp::ReadImage(&ho_Image, "image.jpg");  //读取图片
	hv_ModelFile = "model.shm";
	HalconCpp::ReadShapeModel(hv_ModelFile, &hv_ReusedModelID);                  //读取模板
	HalconCpp::GetShapeModelContours(&ho_ReusedShapeModel, hv_ReusedModelID, 1);               //返回一个轮廓模型的轮廓表示
	HalconCpp::GetShapeModelOrigin(hv_ReusedModelID, &hv_ReusedRefPointRow, &hv_ReusedRefPointCol);                //返回一个轮廓模型的原点位置（hv_ReusedRefPointRow，hv_ReusedRefPointCol）
	HalconCpp::GetShapeModelParams(hv_ReusedModelID, &hv_NumLevels, &hv_AngleStart, &hv_AngleExtent,
		&hv_AngleStep, &hv_ScaleMin, &hv_ScaleMax, &hv_ScaleStep, &hv_Metric, &hv_MinContrast);                      //返回一个轮廓模型的参数

	try{
		//进行匹配
//		HalconCpp::FindScaledShapeModel(ho_Image, hv_ReusedModelID, hv_AngleStart, hv_AngleExtent,
//			hv_ScaleMin, hv_ScaleMax, 0.65, 2, 0, "least_squares", 0, 0.8, &hv_RowCheck,
//			&hv_ColumnCheck, &hv_AngleCheck, &hv_ScaleCheck, &hv_Score);                    //在一个图像中找出一个尺度不变轮廓模型的最佳匹配
		HalconCpp::FindScaledShapeModel(ho_Image, hv_ReusedModelID, HTuple(0).TupleRad(), HTuple(360).TupleRad(),
			0.8, 1.5, 0.5, 0, 0.5, "least_squares", 5, 0.8, &hv_RowCheck,
			&hv_ColumnCheck, &hv_AngleCheck, &hv_ScaleCheck, &hv_Score);
	}
	catch (HalconCpp::HException &except)
	{
		except.ErrorNumber();
	}
	HalconCpp::OpenWindow(0, 0, 1624, 1234, 0, "invisible",
		"", &hv_WindowHandle);
	HalconCpp::GenEmptyObj(&ho_FoundContour);                                        //创建一个空的目标元组
	HalconCpp::HTuple end_val39 = (hv_Score.TupleLength()) - 1;
	HalconCpp::HTuple step_val39 = 1;
	HalconCpp::DispImage(ho_Image, hv_WindowHandle);
	set_display_font(hv_WindowHandle, 25, "mono", "true", "false");
	for (hv_i = 0; hv_i.Continue(end_val39, step_val39); hv_i += step_val39)
	{
		HalconCpp::VectorAngleToRigid(hv_ReusedRefPointRow, hv_ReusedRefPointCol, 0, HalconCpp::HTuple(hv_RowCheck[hv_i]),          //从点和角度方面计算一个严格的仿射变换
			HalconCpp::HTuple(hv_ColumnCheck[hv_i]), HalconCpp::HTuple(hv_AngleCheck[hv_i]), &hv_MovementOfObject);
		HalconCpp::HomMat2dScale(hv_MovementOfObject, HalconCpp::HTuple(hv_ScaleCheck[hv_i]), HalconCpp::HTuple(hv_ScaleCheck[hv_i]),
			HalconCpp::HTuple(hv_RowCheck[hv_i]), HalconCpp::HTuple(hv_ColumnCheck[hv_i]), &hv_MoveAndScalingOfObject);             //为一个同质二维变换矩阵添加一个缩放
		HalconCpp::AffineTransContourXld(ho_ReusedShapeModel, &ho_ModelAtNewPosition, hv_MoveAndScalingOfObject);                    //对XLD轮廓(contour)进行一个任意(旋转和平移)二维仿射变换
		hv_Row = ((const HalconCpp::HTuple&)hv_RowCheck)[hv_i];
		hv_Column = ((const HalconCpp::HTuple&)hv_ColumnCheck)[hv_i];
		HalconCpp::ConcatObj(ho_FoundContour, ho_ModelAtNewPosition, &ho_FoundContour);                                              //连接两个目标元组的图标
		disp_message(hv_WindowHandle, hv_i + 1, "window", hv_Row, hv_Column, "black", "true");
	}
	//HalconCpp::ClearShapeModel(hv_ModelID);                          //清理模板
	//HalconCpp::DispImage(ho_Image, hv_WindowHandle);
	HalconCpp::SetColor(hv_WindowHandle, "magenta");
	HalconCpp::SetLineWidth(hv_WindowHandle, 5);

	HalconCpp::DispXld(ho_FoundContour, hv_WindowHandle);             //没什么作用                   
	HalconCpp::DumpWindow(hv_WindowHandle, "png", "ResultSaved5");
	HalconCpp::CloseWindow(hv_WindowHandle);



	hv_ObjectRow.Clear();
	hv_ObjectRow[0] = 606;
	hv_ObjectRow[1] = 660;
	hv_ObjectRow[2] = 714;
	hv_ObjectRow[3] = 661;
	hv_ObjectCol.Clear();
	hv_ObjectCol[0] = 788;
	hv_ObjectCol[1] = 845;
	hv_ObjectCol[2] = 786;
	hv_ObjectCol[3] = 732;
	HalconCpp::GenCrossContourXld(&ho_Cross, hv_ObjectRow, hv_ObjectCol, 6, 0.785398);                               //根据每个输入点交叉的形状创键一个XLD轮廓(contour)
	hv_WorldX = (((HalconCpp::HTuple(0).Append(10.31)).Append(0)).Append(-10.31)) / 1000.0;
	hv_WorldY = (((HalconCpp::HTuple(-10.31).Append(0)).Append(10.31)).Append(0)) / 1000.0;

	//     if (HDevWindowStack::IsOpen())
	//       DispObj(ho_Image, HDevWindowStack::GetActive());

	hv_CamParam.Clear();           //  相机内参
	hv_CamParam[0] = 0.00873011;    //f
	hv_CamParam[1] = -1664.49;       //k
	hv_CamParam[2] = 5.58794e-006;//sx
	hv_CamParam[3] = 5.6e-006;    //sy
	hv_CamParam[4] = 343.325;     //cx828.653;
	hv_CamParam[5] = 236.872;     //cy633.460;
	hv_CamParam[6] = 656;         //
	hv_CamParam[7] = 492;         //

	//The Z coordinate of the points is set to 0.0 as it is a planar object.
	hv_WorldZ.Clear();
	hv_WorldZ[0] = 0.0;
	hv_WorldZ[1] = 0.0;
	hv_WorldZ[2] = 0.0;
	hv_WorldZ[3] = 0.0;

	//Given the 3D-2D correspondences the plane of the object is extracted with vector_to_pose.
	HalconCpp::VectorToPose(hv_WorldX, hv_WorldY, hv_WorldZ, hv_ObjectRow, hv_ObjectCol, hv_CamParam,
		"planar_analytic_svd", "error", &hv_PoseModelPlane, &hv_Quality);                 // 计算世界坐标系中的绝对位置

	if (hv_RowCheck.Length() != 0)                                                        //匹配成功
	{
		HalconCpp::ImagePointsToWorldPlane(hv_CamParam, hv_PoseModelPlane, hv_RowCheck[0], hv_ColumnCheck[0], "m",
			&hv_X, &hv_Y);                                                                //  将图像中的点转换到世界坐标轴平面Z为零上
		double hv_Angle = hv_AngleCheck * 180 / 3.1415926;                                //弧度转化为角度
		poseLocate = genPose((double)hv_X * 1000, (double)hv_Y * 1000,
			0, 0, 0, hv_Angle);                                                           // poseLocate:三维物体相对相机的位姿
		return 1;
	}
	else                                                                                  //匹配失败
		return -1;
}

void Locate2::set_display_font(HalconCpp::HTuple hv_WindowHandle, HalconCpp::HTuple hv_Size, HalconCpp::HTuple hv_Font, HalconCpp::HTuple hv_Bold,
	HalconCpp::HTuple hv_Slant)
{
	// Local control variables
	HalconCpp::HTuple  hv_OS, hv_Exception, hv_BoldString, hv_SlantString;
	HalconCpp::HTuple  hv_AllowedFontSizes, hv_Distances, hv_Indices, hv_Fonts;
	HalconCpp::HTuple  hv_FontSelRegexp, hv_FontsCourier;

	HalconCpp::GetSystem("operating_system", &hv_OS);
	// dev_get_preferences(...); only in hdevelop
	// dev_set_preferences(...); only in hdevelop
	if (0 != (HalconCpp::HTuple(hv_Size == HalconCpp::HTuple()).TupleOr(hv_Size == -1)))
	{
		hv_Size = 16;
	}
	if (0 != ((hv_OS.TupleSubstr(0, 2)) == HalconCpp::HTuple("Win")))
	{
		//Set font on Windows systems
		if (0 != (HalconCpp::HTuple(HalconCpp::HTuple(hv_Font == HalconCpp::HTuple("mono")).TupleOr(hv_Font == HalconCpp::HTuple("Courier"))).TupleOr(hv_Font == HalconCpp::HTuple("courier"))))
		{
			hv_Font = "Courier New";
		}
		else if (0 != (hv_Font == HalconCpp::HTuple("sans")))
		{
			hv_Font = "Arial";
		}
		else if (0 != (hv_Font == HalconCpp::HTuple("serif")))
		{
			hv_Font = "Times New Roman";
		}
		if (0 != (hv_Bold == HalconCpp::HTuple("true")))
		{
			hv_Bold = 1;
		}
		else if (0 != (hv_Bold == HalconCpp::HTuple("false")))
		{
			hv_Bold = 0;
		}
		else
		{
			hv_Exception = "Wrong value of control parameter Bold";
			throw HalconCpp::HException(hv_Exception);
		}
		if (0 != (hv_Slant == HalconCpp::HTuple("true")))
		{
			hv_Slant = 1;
		}
		else if (0 != (hv_Slant == HalconCpp::HTuple("false")))
		{
			hv_Slant = 0;
		}
		else
		{
			hv_Exception = "Wrong value of control parameter Slant";
			throw HalconCpp::HException(hv_Exception);
		}
		try
		{
			HalconCpp::SetFont(hv_WindowHandle, ((((((("-" + hv_Font) + "-") + hv_Size) + "-*-") + hv_Slant) + "-*-*-") + hv_Bold) + "-");
		}
		// catch (Exception)
		catch (HalconCpp::HException &HDevExpDefaultException)
		{
			HDevExpDefaultException.ToHTuple(&hv_Exception);
			//throw (Exception)
		}
	}
	else if (0 != ((hv_OS.TupleSubstr(0, 2)) == HalconCpp::HTuple("Dar")))
	{
		//Set font on Mac OS X systems
		if (0 != (hv_Bold == HalconCpp::HTuple("true")))
		{
			hv_BoldString = "Bold";
		}
		else if (0 != (hv_Bold == HalconCpp::HTuple("false")))
		{
			hv_BoldString = "";
		}
		else
		{
			hv_Exception = "Wrong value of control parameter Bold";
			throw HalconCpp::HException(hv_Exception);
		}
		if (0 != (hv_Slant == HalconCpp::HTuple("true")))
		{
			hv_SlantString = "Italic";
		}
		else if (0 != (hv_Slant == HalconCpp::HTuple("false")))
		{
			hv_SlantString = "";
		}
		else
		{
			hv_Exception = "Wrong value of control parameter Slant";
			throw HalconCpp::HException(hv_Exception);
		}
		if (0 != (HalconCpp::HTuple(HalconCpp::HTuple(hv_Font == HalconCpp::HTuple("mono")).TupleOr(hv_Font == HalconCpp::HTuple("Courier"))).TupleOr(hv_Font == HalconCpp::HTuple("courier"))))
		{
			hv_Font = "CourierNewPS";
		}
		else if (0 != (hv_Font == HalconCpp::HTuple("sans")))
		{
			hv_Font = "Arial";
		}
		else if (0 != (hv_Font == HalconCpp::HTuple("serif")))
		{
			hv_Font = "TimesNewRomanPS";
		}
		if (0 != (HalconCpp::HTuple(hv_Bold == HalconCpp::HTuple("true")).TupleOr(hv_Slant == HalconCpp::HTuple("true"))))
		{
			hv_Font = ((hv_Font + "-") + hv_BoldString) + hv_SlantString;
		}
		hv_Font += HalconCpp::HTuple("MT");
		try
		{
			SetFont(hv_WindowHandle, (hv_Font + "-") + hv_Size);
		}
		// catch (Exception)
		catch (HalconCpp::HException &HDevExpDefaultException)
		{
			HDevExpDefaultException.ToHTuple(&hv_Exception);
			//throw (Exception)
		}
	}
	else
	{
		//Set font for UNIX systems
		hv_Size = hv_Size*1.25;
		hv_AllowedFontSizes.Clear();
		hv_AllowedFontSizes[0] = 11;
		hv_AllowedFontSizes[1] = 14;
		hv_AllowedFontSizes[2] = 17;
		hv_AllowedFontSizes[3] = 20;
		hv_AllowedFontSizes[4] = 25;
		hv_AllowedFontSizes[5] = 34;
		if (0 != ((hv_AllowedFontSizes.TupleFind(hv_Size)) == -1))
		{
			hv_Distances = (hv_AllowedFontSizes - hv_Size).TupleAbs();
			TupleSortIndex(hv_Distances, &hv_Indices);
			hv_Size = ((const HalconCpp::HTuple&)hv_AllowedFontSizes)[HalconCpp::HTuple(hv_Indices[0])];
		}
		if (0 != (HalconCpp::HTuple(hv_Font == HalconCpp::HTuple("mono")).TupleOr(hv_Font == HalconCpp::HTuple("Courier"))))
		{
			hv_Font = "courier";
		}
		else if (0 != (hv_Font == HalconCpp::HTuple("sans")))
		{
			hv_Font = "helvetica";
		}
		else if (0 != (hv_Font == HalconCpp::HTuple("serif")))
		{
			hv_Font = "times";
		}
		if (0 != (hv_Bold == HalconCpp::HTuple("true")))
		{
			hv_Bold = "bold";
		}
		else if (0 != (hv_Bold == HalconCpp::HTuple("false")))
		{
			hv_Bold = "medium";
		}
		else
		{
			hv_Exception = "Wrong value of control parameter Bold";
			throw HalconCpp::HException(hv_Exception);
		}
		if (0 != (hv_Slant == HalconCpp::HTuple("true")))
		{
			if (0 != (hv_Font == HalconCpp::HTuple("times")))
			{
				hv_Slant = "i";
			}
			else
			{
				hv_Slant = "o";
			}
		}
		else if (0 != (hv_Slant == HalconCpp::HTuple("false")))
		{
			hv_Slant = "r";
		}
		else
		{
			hv_Exception = "Wrong value of control parameter Slant";
			throw HalconCpp::HException(hv_Exception);
		}
		try
		{
			SetFont(hv_WindowHandle, ((((((("-adobe-" + hv_Font) + "-") + hv_Bold) + "-") + hv_Slant) + "-normal-*-") + hv_Size) + "-*-*-*-*-*-*-*");
		}
		// catch (Exception)
		catch (HalconCpp::HException &HDevExpDefaultException)
		{
			HDevExpDefaultException.ToHTuple(&hv_Exception);
			if (0 != (HalconCpp::HTuple((hv_OS.TupleSubstr(0, 4)) == HalconCpp::HTuple("Linux")).TupleAnd(hv_Font == HalconCpp::HTuple("courier"))))
			{
				QueryFont(hv_WindowHandle, &hv_Fonts);
				hv_FontSelRegexp = (("^-[^-]*-[^-]*[Cc]ourier[^-]*-" + hv_Bold) + "-") + hv_Slant;
				hv_FontsCourier = (hv_Fonts.TupleRegexpSelect(hv_FontSelRegexp)).TupleRegexpMatch(hv_FontSelRegexp);
				if (0 != ((hv_FontsCourier.TupleLength()) == 0))
				{
					hv_Exception = "Wrong font name";
					//throw (Exception)
				}
				else
				{
					try
					{
						HalconCpp::SetFont(hv_WindowHandle, ((HalconCpp::HTuple(hv_FontsCourier[0]) + "-normal-*-") + hv_Size) + "-*-*-*-*-*-*-*");
					}
					// catch (Exception)
					catch (HalconCpp::HException &HDevExpDefaultException)
					{
						HDevExpDefaultException.ToHTuple(&hv_Exception);
						//throw (Exception)
					}
				}
			}
			//throw (Exception)
		}
	}
	// dev_set_preferences(...); only in hdevelop
	return;
}

void Locate2::disp_message(HalconCpp::HTuple hv_WindowHandle, HalconCpp::HTuple hv_String, HalconCpp::HTuple hv_CoordSystem,
	HalconCpp::HTuple hv_Row, HalconCpp::HTuple hv_Column, HalconCpp::HTuple hv_Color, HalconCpp::HTuple hv_Box)
{
	// Local control variables
	HalconCpp::HTuple  hv_Red, hv_Green, hv_Blue, hv_Row1Part;
	HalconCpp::HTuple  hv_Column1Part, hv_Row2Part, hv_Column2Part, hv_RowWin;
	HalconCpp::HTuple  hv_ColumnWin, hv_WidthWin, hv_HeightWin, hv_MaxAscent;
	HalconCpp::HTuple  hv_MaxDescent, hv_MaxWidth, hv_MaxHeight, hv_R1;
	HalconCpp::HTuple  hv_C1, hv_FactorRow, hv_FactorColumn, hv_Width;
	HalconCpp::HTuple  hv_Index, hv_Ascent, hv_Descent, hv_W, hv_H, hv_FrameHeight;
	HalconCpp::HTuple  hv_FrameWidth, hv_R2, hv_C2, hv_DrawMode, hv_Exception;
	HalconCpp::HTuple  hv_CurrentColor;

	//     HalconCpp::OpenWindow(0, 0, 1624, 1234, 0, "invisible",
	//                           "", &hv_WindowHandle);
	//prepare window
	HalconCpp::GetRgb(hv_WindowHandle, &hv_Red, &hv_Green, &hv_Blue);
	HalconCpp::GetPart(hv_WindowHandle, &hv_Row1Part, &hv_Column1Part, &hv_Row2Part, &hv_Column2Part);
	HalconCpp::GetWindowExtents(hv_WindowHandle, &hv_RowWin, &hv_ColumnWin, &hv_WidthWin, &hv_HeightWin);
	HalconCpp::SetPart(hv_WindowHandle, 0, 0, hv_HeightWin - 1, hv_WidthWin - 1);
	//
	//default settings
	if (0 != (hv_Row == -1))
	{
		hv_Row = 12;
	}
	if (0 != (hv_Column == -1))
	{
		hv_Column = 12;
	}
	if (0 != (hv_Color == HalconCpp::HTuple()))
	{
		hv_Color = "";
	}
	//
	hv_String = (("" + hv_String) + "").TupleSplit("\n");
	//
	//Estimate extentions of text depending on font size.
	HalconCpp::GetFontExtents(hv_WindowHandle, &hv_MaxAscent, &hv_MaxDescent, &hv_MaxWidth, &hv_MaxHeight);
	if (0 != (hv_CoordSystem == HalconCpp::HTuple("window")))
	{
		hv_R1 = hv_Row;
		hv_C1 = hv_Column;
	}
	else
	{
		//transform image to window coordinates
		hv_FactorRow = (1.*hv_HeightWin) / ((hv_Row2Part - hv_Row1Part) + 1);
		hv_FactorColumn = (1.*hv_WidthWin) / ((hv_Column2Part - hv_Column1Part) + 1);
		hv_R1 = ((hv_Row - hv_Row1Part) + 0.5)*hv_FactorRow;
		hv_C1 = ((hv_Column - hv_Column1Part) + 0.5)*hv_FactorColumn;
	}
	//
	//display text box depending on text size
	if (0 != (hv_Box == HalconCpp::HTuple("true")))
	{
		//calculate box extents
		hv_String = (" " + hv_String) + " ";
		hv_Width = HalconCpp::HTuple();
		HalconCpp::HTuple end_val57 = (hv_String.TupleLength()) - 1;
		HalconCpp::HTuple step_val57 = 1;
		for (hv_Index = 0; hv_Index.Continue(end_val57, step_val57); hv_Index += step_val57)
		{
			HalconCpp::GetStringExtents(hv_WindowHandle, HalconCpp::HTuple(hv_String[hv_Index]), &hv_Ascent,
				&hv_Descent, &hv_W, &hv_H);
			hv_Width = hv_Width.TupleConcat(hv_W);
		}
		hv_FrameHeight = hv_MaxHeight*(hv_String.TupleLength());
		hv_FrameWidth = (HalconCpp::HTuple(0).TupleConcat(hv_Width)).TupleMax();
		hv_R2 = hv_R1 + hv_FrameHeight;
		hv_C2 = hv_C1 + hv_FrameWidth;
		/* hv_R2 = hv_R1+30;
		hv_C2 = hv_C1+30*/;
		//display rectangles
		HalconCpp::GetDraw(hv_WindowHandle, &hv_DrawMode);
		HalconCpp::SetDraw(hv_WindowHandle, "fill");
		HalconCpp::SetColor(hv_WindowHandle, "light gray");
		HalconCpp::DispRectangle1(hv_WindowHandle, hv_R1 + 3, hv_C1 + 3, hv_R2 + 3, hv_C2 + 3);
		HalconCpp::SetColor(hv_WindowHandle, "white");
		HalconCpp::DispRectangle1(hv_WindowHandle, hv_R1, hv_C1, hv_R2, hv_C2);
		HalconCpp::SetDraw(hv_WindowHandle, hv_DrawMode);
	}
	else if (0 != (hv_Box != HalconCpp::HTuple("false")))
	{
		hv_Exception = "Wrong value of control parameter Box";
		throw HalconCpp::HException(hv_Exception);
	}
	//Write text.
	HalconCpp::HTuple end_val78 = (hv_String.TupleLength()) - 1;
	HalconCpp::HTuple step_val78 = 1;
	for (hv_Index = 0; hv_Index.Continue(end_val78, step_val78); hv_Index += step_val78)
	{
		hv_CurrentColor = ((const HalconCpp::HTuple&)hv_Color)[hv_Index % (hv_Color.TupleLength())];
		if (0 != (HalconCpp::HTuple(hv_CurrentColor != HalconCpp::HTuple("")).TupleAnd(hv_CurrentColor != HalconCpp::HTuple("auto"))))
		{
			HalconCpp::SetColor(hv_WindowHandle, hv_CurrentColor);
		}
		else
		{
			HalconCpp::SetRgb(hv_WindowHandle, hv_Red, hv_Green, hv_Blue);
		}
		hv_Row = hv_R1 + (hv_MaxHeight*hv_Index);
		HalconCpp::SetTposition(hv_WindowHandle, hv_Row, hv_C1);
		HalconCpp::WriteString(hv_WindowHandle, HalconCpp::HTuple(hv_String[hv_Index]));
	}
	//reset changed window settings
	HalconCpp::SetRgb(hv_WindowHandle, hv_Red, hv_Green, hv_Blue);
	HalconCpp::SetPart(hv_WindowHandle, hv_Row1Part, hv_Column1Part, hv_Row2Part, hv_Column2Part);
	return;
}

void Locate2::saveResult()
{

}

Locate3::Locate3()
{
	isModelLoaded = false;
}

int Locate3::loadModel()
{
	// Local control variables
	if (!isModelLoaded)
	{
		HalconCpp::HTuple hv_ErrorVar;
		try
		{
			hv_ErrorVar = 2;

			HalconCpp::ReadDeformableModel("peg-dfm.dfm", &hv_ModelID);
			//            HalconCpp::ReadDeformableModel("peg-top-template.dfm", &hv_ModelID);
		}
		catch (HalconCpp::HException e)
		{
			hv_ErrorVar = (int)e.ErrorNumber();
			if (hv_ErrorVar < 0)
				throw e;
			return -1;
		}
	}

	return 1;
}

int Locate3::locate(Pose &poseLocate)
{
	HalconCpp::HObject  ho_Image;
	HalconCpp::ReadImage(&ho_Image, "cache.png");
	HalconCpp::HTuple hv_CovPose, hv_Score;
	try{
		HalconCpp::FindPlanarCalibDeformableModel(ho_Image, hv_ModelID,
			0, 0.39,//Start and extand angle
			0.95, 1.05, //Row scale min and max
			0.95, 1.05, //Column scale min and max
			0.8,  //Min score
			1,    //Number matches
			1,    //Max overlap
			0,    //Number levels
			0.1,  //Greediness
			HalconCpp::HTuple(),
			HalconCpp::HTuple(),
			&hv_Pose, &hv_CovPose, &hv_Score);
		//        HalconCpp::FindPlanarCalibDeformableModel(ho_Image, hv_ModelID,
		//                                                  -3.14, 6.29,//Start and extand angle
		//                                                  0.5, 1.0, //Row scale min and max
		//                                                  0.5, 1.0, //Column scale min and max
		//                                                  0.3,  //Min score
		//                                                  0,    //Number matches
		//                                                  0,    //Max overlap
		//                                                  0,    //Number levels
		//                                                  0,  //Greediness
		//                                                  HalconCpp::HTuple(),
		//                                                  HalconCpp::HTuple(),
		//                                                  &hv_Pose,&hv_CovPose,&hv_Score);
	}
	catch (HalconCpp::HException &except)
	{
		except.ErrorNumber();
	}
	HalconCpp::ClearShapeModel(hv_ModelID);                  //清理模板
	if (hv_Pose.Length() != 0)
	{
		poseLocate = genPose((double)hv_Pose[0] * 1000, (double)hv_Pose[1] * 1000,
			(double)hv_Pose[2] * 1000, (double)hv_Pose[3], (double)hv_Pose[4],
			(double)hv_Pose[5]);
		poseLocate.rx = poseLocate.rx>180 ? poseLocate.rx - 360 : poseLocate.rx;
		poseLocate.ry = poseLocate.ry>180 ? poseLocate.ry - 360 : poseLocate.ry;
		poseLocate.rz = poseLocate.rz>180 ? poseLocate.rz - 360 : poseLocate.rz;

		//保存定位结果
		saveResult();
		return 1;
	}
	else
		return -1;
}

void Locate3::saveResult()
{
	// Local iconic variables
	HalconCpp::HObject  ho_Image, ho_ModelContours, ho_ContoursAffinTrans,
		ho_ContoursTrans, ho_FoundContour;
	HalconCpp::HObject  ho_ObjectSelected, ho_ModelWorld;

	// Local control variables
	HalconCpp::HTuple  hv_CamParam;
	HalconCpp::HTuple  hv_WindowDrawResult;
	HalconCpp::HTuple  hv_HomMat3D, hv_NumberContour;
	HalconCpp::HTuple  hv_K, hv_Y, hv_X, hv_Z, hv_Xc, hv_Yc, hv_Zc, hv_R;
	HalconCpp::HTuple  hv_C;
	HalconCpp::HTuple hv_ModelRow, hv_ModelCol, hv_CamParNoRad, hv_ModelPose;

	hv_CamParam.Clear();
	hv_CamParam[0] = 0.00840228644304546;
	hv_CamParam[1] = -1019.55707294893;
	hv_CamParam[2] = 4.40722435287872e-006;
	hv_CamParam[3] = 4.4e-006;
	hv_CamParam[4] = 828.65339497083;
	hv_CamParam[5] = 633.46017521628;
	hv_CamParam[6] = 1624;
	hv_CamParam[7] = 1234;

	HalconCpp::ReadImage(&ho_Image, "cache.png");
	
	HalconCpp::GetDeformableModelContours(&ho_ModelContours, hv_ModelID, 1);     //获取模板轮廓
	HalconCpp::GetDeformableModelParams(hv_ModelID, "model_row", &hv_ModelRow);  //获取模板行
	HalconCpp::GetDeformableModelParams(hv_ModelID, "model_col", &hv_ModelCol);  //获取模板列
	HalconCpp::GetDeformableModelParams(hv_ModelID, "model_pose", &hv_ModelPose);//获取模板位姿
	HalconCpp::ChangeRadialDistortionCamPar("preserve_resolution", hv_CamParam, 0, &hv_CamParNoRad);//求出理想无畸变内参、由相机内参自标定求出相机外外参
	HalconCpp::AffineTransContourXld(ho_ModelContours, &ho_ContoursAffinTrans,
		(((HalconCpp::HTuple(1).Append(0))                                      //对轮廓进行某种（平移和缩放）变换
		.TupleConcat(hv_ModelRow)).
		TupleConcat((HalconCpp::HTuple(0).Append(1))))
		.TupleConcat(hv_ModelCol));
	HalconCpp::ContourToWorldPlaneXld(ho_ContoursAffinTrans, &ho_ContoursTrans,
		hv_CamParNoRad, hv_ModelPose, "m");                                   //将通过相机内参，将轮廓转换到世界坐标系hv_ModelPose
	HalconCpp::CountObj(ho_ContoursTrans, &hv_NumberContour);                 //计算Region中有几个单独的区域

	HalconCpp::GenEmptyObj(&ho_FoundContour);                                 //

	HalconCpp::HTuple end_val40 = hv_NumberContour;
	HalconCpp::HTuple step_val40 = 1;
	for (hv_K = 1; hv_K.Continue(end_val40, step_val40); hv_K += step_val40)
	{
		HalconCpp::SelectObj(ho_ContoursTrans, &ho_ObjectSelected, hv_K);                       //从一个目标元组中选择目标
		HalconCpp::GetContourXld(ho_ObjectSelected, &hv_Y, &hv_X);                             //返回XLD轮廓(contour)的坐标
		hv_Z = HalconCpp::HTuple(hv_X.TupleLength(), 0.0);                                     //确定z坐标
		//Transform the metric model into world coordinates.
		HalconCpp::PoseToHomMat3d(hv_Pose, &hv_HomMat3D);                                       //将一个三位模式转换为一个同质变换矩阵
		HalconCpp::AffineTransPoint3d(hv_HomMat3D, hv_X, hv_Y, hv_Z, &hv_Xc, &hv_Yc, &hv_Zc);   //对点运用一个随即仿射三维变换
		//Project the 3D points of the model, which are
		//available as world coordinates, into the image
		//coordinate system.
		HalconCpp::Project3dPoint(hv_Xc, hv_Yc, hv_Zc, hv_CamParam, &hv_R, &hv_C);  //将三维点投射到子像素图像坐标(hvR,hvC)
		HalconCpp::GenContourPolygonXld(&ho_ModelWorld, hv_R, hv_C);                //根据一个多边形（polygon）（以元组形式给出）创建一个XLD轮廓(contour)
		HalconCpp::ConcatObj(ho_FoundContour, ho_ModelWorld, &ho_FoundContour);     //连接两个目标元组的图标
	}
	HalconCpp::OpenWindow(0, 0, 1624, 1234, 0, "invisible",
		"", &hv_WindowDrawResult);                          //打开窗口
	HalconCpp::DispImage(ho_Image, hv_WindowDrawResult);    //在窗口显示图片
	HalconCpp::SetColor(hv_WindowDrawResult, "magenta");    //设置颜色
	HalconCpp::SetLineWidth(hv_WindowDrawResult, 5);        //设置线宽
	HalconCpp::DispXld(ho_FoundContour, hv_WindowDrawResult);//显示轮廓
	HalconCpp::DumpWindow(hv_WindowDrawResult, "png", "ResultSaved3");    //把窗口内容写入文件ResultSaved3
	HalconCpp::CloseWindow(hv_WindowDrawResult);           //关闭窗口
}
Locate4::Locate4()
{
	isModelLoaded = false;
}

int Locate4::loadModel()                        //载入模板
{
	// Local control variables
	if (!isModelLoaded)
	{
		HalconCpp::HTuple hv_ErrorVar;
		try
		{
			hv_ErrorVar = 2;

			HalconCpp::ReadDeformableModel("line-circle_template.dfm", &hv_ModelID);

		}
		catch (HalconCpp::HException e)
		{
			hv_ErrorVar = (int)e.ErrorNumber();
			if (hv_ErrorVar < 0)
				throw e;
			return -1;
		}
	}

	return 1;
}

int Locate4::locate(Pose &poseLocate)        //定位函数
{
	// Local iconic variables     图像参数
	HalconCpp::HObject  ho_ReusedShapeModel, ho_Cross, ho_Image, ho_FoundContour;
	HalconCpp::HObject  ho_ModelAtNewPosition;
	// Local control variables   控制参数
	HalconCpp::HTuple  hv_WindowHandle, hv_ModelFile, hv_ReusedModelID;
	HalconCpp::HTuple  hv_ReusedRefPointRow, hv_ReusedRefPointCol, hv_NumLevels;
	HalconCpp::HTuple  hv_AngleStart, hv_AngleExtent, hv_AngleStep, hv_ScaleMin;
	HalconCpp::HTuple  hv_ScaleMax, hv_ScaleStep, hv_Metric, hv_MinContrast;
	HalconCpp::HTuple  hv_ObjectRow, hv_ObjectCol, hv_WorldX, hv_WorldY;
	HalconCpp::HTuple  hv_CamParam, hv_WorldZ, hv_PoseModelPlane, hv_Quality;
	HalconCpp::HTuple  hv_RowCheck, hv_ColumnCheck, hv_AngleCheck, hv_ScaleCheck;
	HalconCpp::HTuple  hv_Score, hv_i, hv_MovementOfObject, hv_MoveAndScalingOfObject;
	HalconCpp::HTuple  hv_Row, hv_Column, hv_X, hv_Y;

	HalconCpp::ReadImage(&ho_Image, "cache.png");                                    //读取图片
	hv_ModelFile = "newline.sbm";
	//hv_ModelFile = "line.sbm";
	HalconCpp::ReadShapeModel(hv_ModelFile, &hv_ReusedModelID);                      //读取模板
	HalconCpp::GetShapeModelContours(&ho_ReusedShapeModel, hv_ReusedModelID, 1);     //返回一个轮廓模型的轮廓表示
	HalconCpp::GetShapeModelOrigin(hv_ReusedModelID, &hv_ReusedRefPointRow, &hv_ReusedRefPointCol);  //返回一个轮廓模型的原点位置（hv_ReusedRefPointRow，hv_ReusedRefPointCol）
	HalconCpp::GetShapeModelParams(hv_ReusedModelID, &hv_NumLevels, &hv_AngleStart, &hv_AngleExtent,
		&hv_AngleStep, &hv_ScaleMin, &hv_ScaleMax, &hv_ScaleStep, &hv_Metric, &hv_MinContrast);      //返回一个轮廓模型的参数
	try{
		HalconCpp::FindScaledShapeModel(ho_Image, hv_ReusedModelID, hv_AngleStart, hv_AngleExtent,  
			hv_ScaleMin, hv_ScaleMax, 0.65, 4, 0, "least_squares", 0, 0.8, &hv_RowCheck,   
			&hv_ColumnCheck, &hv_AngleCheck, &hv_ScaleCheck, &hv_Score);    //在一个图像中找出一个尺度不变轮廓模型的最佳匹配
	}
	catch (HalconCpp::HException &except)
	{
		except.ErrorNumber();
	}

	HalconCpp::OpenWindow(0, 0, 1624, 1234, 0, "invisible",
		"", &hv_WindowHandle);
	HalconCpp::GenEmptyObj(&ho_FoundContour);                      //创建一个空的目标元组
	HalconCpp::HTuple end_val39 = (hv_Score.TupleLength()) - 1;
	HalconCpp::HTuple step_val39 = 1;
	HalconCpp::DispImage(ho_Image, hv_WindowHandle);
	set_display_font(hv_WindowHandle, 25, "mono", "true", "false");
	for (hv_i = 0; hv_i.Continue(end_val39, step_val39); hv_i += step_val39)
	{
		HalconCpp::VectorAngleToRigid(hv_ReusedRefPointRow, hv_ReusedRefPointCol, 0, HalconCpp::HTuple(hv_RowCheck[hv_i]),           //从点和角度方面计算一个严格的仿射变换
			HalconCpp::HTuple(hv_ColumnCheck[hv_i]), HalconCpp::HTuple(hv_AngleCheck[hv_i]), &hv_MovementOfObject);                      
		HalconCpp::HomMat2dScale(hv_MovementOfObject, HalconCpp::HTuple(hv_ScaleCheck[hv_i]), HalconCpp::HTuple(hv_ScaleCheck[hv_i]),   
			HalconCpp::HTuple(hv_RowCheck[hv_i]), HalconCpp::HTuple(hv_ColumnCheck[hv_i]), &hv_MoveAndScalingOfObject);              //为一个同质二维变换矩阵添加一个缩放
		HalconCpp::AffineTransContourXld(ho_ReusedShapeModel, &ho_ModelAtNewPosition, hv_MoveAndScalingOfObject);                    //对XLD轮廓(contour)进行一个任意(旋转和平移)二维仿射变换
		hv_Row = ((const HalconCpp::HTuple&)hv_RowCheck)[hv_i];
		hv_Column = ((const HalconCpp::HTuple&)hv_ColumnCheck)[hv_i];
		HalconCpp::ConcatObj(ho_FoundContour, ho_ModelAtNewPosition, &ho_FoundContour);                                            //连接两个目标元组的图标
		disp_message(hv_WindowHandle, hv_i + 1, "window", hv_Row, hv_Column, "black", "true");
	}

	//HalconCpp::DispImage(ho_Image, hv_WindowHandle);
	HalconCpp::SetColor(hv_WindowHandle, "magenta");
	HalconCpp::SetLineWidth(hv_WindowHandle, 5);
	HalconCpp::DispXld(ho_FoundContour, hv_WindowHandle);
	HalconCpp::DumpWindow(hv_WindowHandle, "png", "ResultSaved4");
	HalconCpp::CloseWindow(hv_WindowHandle);

	hv_ObjectRow.Clear();
	hv_ObjectRow[0] = 606;
	hv_ObjectRow[1] = 660;
	hv_ObjectRow[2] = 714;
	hv_ObjectRow[3] = 661;
	hv_ObjectCol.Clear();
	hv_ObjectCol[0] = 788;
	hv_ObjectCol[1] = 845;
	hv_ObjectCol[2] = 786;
	hv_ObjectCol[3] = 732;
	HalconCpp::GenCrossContourXld(&ho_Cross, hv_ObjectRow, hv_ObjectCol, 6, 0.785398);      //根据每个输入点交叉的形状创键一个XLD轮廓(contour)
	hv_WorldX = (((HalconCpp::HTuple(0).Append(10.31)).Append(0)).Append(-10.31)) / 1000.0;
	hv_WorldY = (((HalconCpp::HTuple(-10.31).Append(0)).Append(10.31)).Append(0)) / 1000.0;

	hv_CamParam.Clear();
	hv_CamParam[0] = 0.00840229;
	hv_CamParam[1] = -1019.56;
	hv_CamParam[2] = 4.40722e-006;
	hv_CamParam[3] = 4.4e-006;
	hv_CamParam[4] = 828.653;
	hv_CamParam[5] = 633.460;
	hv_CamParam[6] = 1624;
	hv_CamParam[7] = 1234;

	//The Z coordinate of the points is set to 0.0 as it is a planar object.
	hv_WorldZ.Clear();
	hv_WorldZ[0] = 0.0;
	hv_WorldZ[1] = 0.0;
	hv_WorldZ[2] = 0.0;
	hv_WorldZ[3] = 0.0;

	//Given the 3D-2D correspondences the plane of the object is extracted with vector_to_pose.
	HalconCpp::VectorToPose(hv_WorldX, hv_WorldY, hv_WorldZ, hv_ObjectRow, hv_ObjectCol, hv_CamParam,
		"planar_analytic_svd", "error", &hv_PoseModelPlane, &hv_Quality);       // 计算世界坐标系中的绝对位置

	if (hv_RowCheck.Length() != 0)
	{
		HalconCpp::ImagePointsToWorldPlane(hv_CamParam, hv_PoseModelPlane, hv_RowCheck[0], hv_ColumnCheck[0], "m",
			&hv_X, &hv_Y);                                                     //  将图像中的点转换到世界坐标轴平面Z为零上
		double hv_Angle = hv_AngleCheck * 180 / 3.1415926;
		poseLocate = genPose((double)hv_X * 1000, (double)hv_Y * 1000,
			0, 0, 0, hv_Angle);
		return 1;
	}
	else
		return -1;
}

void Locate4::set_display_font(HalconCpp::HTuple hv_WindowHandle, HalconCpp::HTuple hv_Size, HalconCpp::HTuple hv_Font, HalconCpp::HTuple hv_Bold,
	HalconCpp::HTuple hv_Slant)
{
	// Local control variables
	HalconCpp::HTuple  hv_OS, hv_Exception, hv_BoldString, hv_SlantString;
	HalconCpp::HTuple  hv_AllowedFontSizes, hv_Distances, hv_Indices, hv_Fonts;
	HalconCpp::HTuple  hv_FontSelRegexp, hv_FontsCourier;

	HalconCpp::GetSystem("operating_system", &hv_OS);
	// dev_get_preferences(...); only in hdevelop
	// dev_set_preferences(...); only in hdevelop
	if (0 != (HalconCpp::HTuple(hv_Size == HalconCpp::HTuple()).TupleOr(hv_Size == -1)))
	{
		hv_Size = 16;
	}
	if (0 != ((hv_OS.TupleSubstr(0, 2)) == HalconCpp::HTuple("Win")))
	{
		//Set font on Windows systems
		if (0 != (HalconCpp::HTuple(HalconCpp::HTuple(hv_Font == HalconCpp::HTuple("mono")).TupleOr(hv_Font == HalconCpp::HTuple("Courier"))).TupleOr(hv_Font == HalconCpp::HTuple("courier"))))
		{
			hv_Font = "Courier New";
		}
		else if (0 != (hv_Font == HalconCpp::HTuple("sans")))
		{
			hv_Font = "Arial";
		}
		else if (0 != (hv_Font == HalconCpp::HTuple("serif")))
		{
			hv_Font = "Times New Roman";
		}
		if (0 != (hv_Bold == HalconCpp::HTuple("true")))
		{
			hv_Bold = 1;
		}
		else if (0 != (hv_Bold == HalconCpp::HTuple("false")))
		{
			hv_Bold = 0;
		}
		else
		{
			hv_Exception = "Wrong value of control parameter Bold";
			throw HalconCpp::HException(hv_Exception);
		}
		if (0 != (hv_Slant == HalconCpp::HTuple("true")))
		{
			hv_Slant = 1;
		}
		else if (0 != (hv_Slant == HalconCpp::HTuple("false")))
		{
			hv_Slant = 0;
		}
		else
		{
			hv_Exception = "Wrong value of control parameter Slant";
			throw HalconCpp::HException(hv_Exception);
		}
		try
		{
			HalconCpp::SetFont(hv_WindowHandle, ((((((("-" + hv_Font) + "-") + hv_Size) + "-*-") + hv_Slant) + "-*-*-") + hv_Bold) + "-");
		}
		// catch (Exception)
		catch (HalconCpp::HException &HDevExpDefaultException)
		{
			HDevExpDefaultException.ToHTuple(&hv_Exception);
			//throw (Exception)
		}
	}
	else if (0 != ((hv_OS.TupleSubstr(0, 2)) == HalconCpp::HTuple("Dar")))
	{
		//Set font on Mac OS X systems
		if (0 != (hv_Bold == HalconCpp::HTuple("true")))
		{
			hv_BoldString = "Bold";
		}
		else if (0 != (hv_Bold == HalconCpp::HTuple("false")))
		{
			hv_BoldString = "";
		}
		else
		{
			hv_Exception = "Wrong value of control parameter Bold";
			throw HalconCpp::HException(hv_Exception);
		}
		if (0 != (hv_Slant == HalconCpp::HTuple("true")))
		{
			hv_SlantString = "Italic";
		}
		else if (0 != (hv_Slant == HalconCpp::HTuple("false")))
		{
			hv_SlantString = "";
		}
		else
		{
			hv_Exception = "Wrong value of control parameter Slant";
			throw HalconCpp::HException(hv_Exception);
		}
		if (0 != (HalconCpp::HTuple(HalconCpp::HTuple(hv_Font == HalconCpp::HTuple("mono")).TupleOr(hv_Font == HalconCpp::HTuple("Courier"))).TupleOr(hv_Font == HalconCpp::HTuple("courier"))))
		{
			hv_Font = "CourierNewPS";
		}
		else if (0 != (hv_Font == HalconCpp::HTuple("sans")))
		{
			hv_Font = "Arial";
		}
		else if (0 != (hv_Font == HalconCpp::HTuple("serif")))
		{
			hv_Font = "TimesNewRomanPS";
		}
		if (0 != (HalconCpp::HTuple(hv_Bold == HalconCpp::HTuple("true")).TupleOr(hv_Slant == HalconCpp::HTuple("true"))))
		{
			hv_Font = ((hv_Font + "-") + hv_BoldString) + hv_SlantString;
		}
		hv_Font += HalconCpp::HTuple("MT");
		try
		{
			SetFont(hv_WindowHandle, (hv_Font + "-") + hv_Size);
		}
		// catch (Exception)
		catch (HalconCpp::HException &HDevExpDefaultException)
		{
			HDevExpDefaultException.ToHTuple(&hv_Exception);
			//throw (Exception)
		}
	}
	else
	{
		//Set font for UNIX systems
		hv_Size = hv_Size*1.25;
		hv_AllowedFontSizes.Clear();
		hv_AllowedFontSizes[0] = 11;
		hv_AllowedFontSizes[1] = 14;
		hv_AllowedFontSizes[2] = 17;
		hv_AllowedFontSizes[3] = 20;
		hv_AllowedFontSizes[4] = 25;
		hv_AllowedFontSizes[5] = 34;
		if (0 != ((hv_AllowedFontSizes.TupleFind(hv_Size)) == -1))
		{
			hv_Distances = (hv_AllowedFontSizes - hv_Size).TupleAbs();
			TupleSortIndex(hv_Distances, &hv_Indices);
			hv_Size = ((const HalconCpp::HTuple&)hv_AllowedFontSizes)[HalconCpp::HTuple(hv_Indices[0])];
		}
		if (0 != (HalconCpp::HTuple(hv_Font == HalconCpp::HTuple("mono")).TupleOr(hv_Font == HalconCpp::HTuple("Courier"))))
		{
			hv_Font = "courier";
		}
		else if (0 != (hv_Font == HalconCpp::HTuple("sans")))
		{
			hv_Font = "helvetica";
		}
		else if (0 != (hv_Font == HalconCpp::HTuple("serif")))
		{
			hv_Font = "times";
		}
		if (0 != (hv_Bold == HalconCpp::HTuple("true")))
		{
			hv_Bold = "bold";
		}
		else if (0 != (hv_Bold == HalconCpp::HTuple("false")))
		{
			hv_Bold = "medium";
		}
		else
		{
			hv_Exception = "Wrong value of control parameter Bold";
			throw HalconCpp::HException(hv_Exception);
		}
		if (0 != (hv_Slant == HalconCpp::HTuple("true")))
		{
			if (0 != (hv_Font == HalconCpp::HTuple("times")))
			{
				hv_Slant = "i";
			}
			else
			{
				hv_Slant = "o";
			}
		}
		else if (0 != (hv_Slant == HalconCpp::HTuple("false")))
		{
			hv_Slant = "r";
		}
		else
		{
			hv_Exception = "Wrong value of control parameter Slant";
			throw HalconCpp::HException(hv_Exception);
		}
		try
		{
			SetFont(hv_WindowHandle, ((((((("-adobe-" + hv_Font) + "-") + hv_Bold) + "-") + hv_Slant) + "-normal-*-") + hv_Size) + "-*-*-*-*-*-*-*");
		}
		// catch (Exception)
		catch (HalconCpp::HException &HDevExpDefaultException)
		{
			HDevExpDefaultException.ToHTuple(&hv_Exception);
			if (0 != (HalconCpp::HTuple((hv_OS.TupleSubstr(0, 4)) == HalconCpp::HTuple("Linux")).TupleAnd(hv_Font == HalconCpp::HTuple("courier"))))
			{
				QueryFont(hv_WindowHandle, &hv_Fonts);
				hv_FontSelRegexp = (("^-[^-]*-[^-]*[Cc]ourier[^-]*-" + hv_Bold) + "-") + hv_Slant;
				hv_FontsCourier = (hv_Fonts.TupleRegexpSelect(hv_FontSelRegexp)).TupleRegexpMatch(hv_FontSelRegexp);
				if (0 != ((hv_FontsCourier.TupleLength()) == 0))
				{
					hv_Exception = "Wrong font name";
					//throw (Exception)
				}
				else
				{
					try
					{
						HalconCpp::SetFont(hv_WindowHandle, ((HalconCpp::HTuple(hv_FontsCourier[0]) + "-normal-*-") + hv_Size) + "-*-*-*-*-*-*-*");
					}
					// catch (Exception)
					catch (HalconCpp::HException &HDevExpDefaultException)
					{
						HDevExpDefaultException.ToHTuple(&hv_Exception);
						//throw (Exception)
					}
				}
			}
			//throw (Exception)
		}
	}
	// dev_set_preferences(...); only in hdevelop
	return;
}

void Locate4::disp_message(HalconCpp::HTuple hv_WindowHandle, HalconCpp::HTuple hv_String, HalconCpp::HTuple hv_CoordSystem,
	HalconCpp::HTuple hv_Row, HalconCpp::HTuple hv_Column, HalconCpp::HTuple hv_Color, HalconCpp::HTuple hv_Box)
{
	// Local control variables
	HalconCpp::HTuple  hv_Red, hv_Green, hv_Blue, hv_Row1Part;
	HalconCpp::HTuple  hv_Column1Part, hv_Row2Part, hv_Column2Part, hv_RowWin;
	HalconCpp::HTuple  hv_ColumnWin, hv_WidthWin, hv_HeightWin, hv_MaxAscent;
	HalconCpp::HTuple  hv_MaxDescent, hv_MaxWidth, hv_MaxHeight, hv_R1;
	HalconCpp::HTuple  hv_C1, hv_FactorRow, hv_FactorColumn, hv_Width;
	HalconCpp::HTuple  hv_Index, hv_Ascent, hv_Descent, hv_W, hv_H, hv_FrameHeight;
	HalconCpp::HTuple  hv_FrameWidth, hv_R2, hv_C2, hv_DrawMode, hv_Exception;
	HalconCpp::HTuple  hv_CurrentColor;

	//     HalconCpp::OpenWindow(0, 0, 1624, 1234, 0, "invisible",
	//                           "", &hv_WindowHandle);
	//prepare window
	HalconCpp::GetRgb(hv_WindowHandle, &hv_Red, &hv_Green, &hv_Blue);
	HalconCpp::GetPart(hv_WindowHandle, &hv_Row1Part, &hv_Column1Part, &hv_Row2Part, &hv_Column2Part);
	HalconCpp::GetWindowExtents(hv_WindowHandle, &hv_RowWin, &hv_ColumnWin, &hv_WidthWin, &hv_HeightWin);
	HalconCpp::SetPart(hv_WindowHandle, 0, 0, hv_HeightWin - 1, hv_WidthWin - 1);
	//
	//default settings
	if (0 != (hv_Row == -1))
	{
		hv_Row = 12;
	}
	if (0 != (hv_Column == -1))
	{
		hv_Column = 12;
	}
	if (0 != (hv_Color == HalconCpp::HTuple()))
	{
		hv_Color = "";
	}
	//
	hv_String = (("" + hv_String) + "").TupleSplit("\n");
	//
	//Estimate extentions of text depending on font size.
	HalconCpp::GetFontExtents(hv_WindowHandle, &hv_MaxAscent, &hv_MaxDescent, &hv_MaxWidth, &hv_MaxHeight);
	if (0 != (hv_CoordSystem == HalconCpp::HTuple("window")))
	{
		hv_R1 = hv_Row;
		hv_C1 = hv_Column;
	}
	else
	{
		//transform image to window coordinates
		hv_FactorRow = (1.*hv_HeightWin) / ((hv_Row2Part - hv_Row1Part) + 1);
		hv_FactorColumn = (1.*hv_WidthWin) / ((hv_Column2Part - hv_Column1Part) + 1);
		hv_R1 = ((hv_Row - hv_Row1Part) + 0.5)*hv_FactorRow;
		hv_C1 = ((hv_Column - hv_Column1Part) + 0.5)*hv_FactorColumn;
	}
	//
	//display text box depending on text size
	if (0 != (hv_Box == HalconCpp::HTuple("true")))
	{
		//calculate box extents
		hv_String = (" " + hv_String) + " ";
		hv_Width = HalconCpp::HTuple();
		HalconCpp::HTuple end_val57 = (hv_String.TupleLength()) - 1;
		HalconCpp::HTuple step_val57 = 1;
		for (hv_Index = 0; hv_Index.Continue(end_val57, step_val57); hv_Index += step_val57)
		{
			HalconCpp::GetStringExtents(hv_WindowHandle, HalconCpp::HTuple(hv_String[hv_Index]), &hv_Ascent,
				&hv_Descent, &hv_W, &hv_H);
			hv_Width = hv_Width.TupleConcat(hv_W);
		}
		hv_FrameHeight = hv_MaxHeight*(hv_String.TupleLength());
		hv_FrameWidth = (HalconCpp::HTuple(0).TupleConcat(hv_Width)).TupleMax();
		hv_R2 = hv_R1 + hv_FrameHeight;
		hv_C2 = hv_C1 + hv_FrameWidth;
		/* hv_R2 = hv_R1+30;
		hv_C2 = hv_C1+30*/;
		//display rectangles
		HalconCpp::GetDraw(hv_WindowHandle, &hv_DrawMode);
		HalconCpp::SetDraw(hv_WindowHandle, "fill");
		HalconCpp::SetColor(hv_WindowHandle, "light gray");
		HalconCpp::DispRectangle1(hv_WindowHandle, hv_R1 + 3, hv_C1 + 3, hv_R2 + 3, hv_C2 + 3);
		HalconCpp::SetColor(hv_WindowHandle, "white");
		HalconCpp::DispRectangle1(hv_WindowHandle, hv_R1, hv_C1, hv_R2, hv_C2);
		HalconCpp::SetDraw(hv_WindowHandle, hv_DrawMode);
	}
	else if (0 != (hv_Box != HalconCpp::HTuple("false")))
	{
		hv_Exception = "Wrong value of control parameter Box";
		throw HalconCpp::HException(hv_Exception);
	}
	//Write text.
	HalconCpp::HTuple end_val78 = (hv_String.TupleLength()) - 1;
	HalconCpp::HTuple step_val78 = 1;
	for (hv_Index = 0; hv_Index.Continue(end_val78, step_val78); hv_Index += step_val78)
	{
		hv_CurrentColor = ((const HalconCpp::HTuple&)hv_Color)[hv_Index % (hv_Color.TupleLength())];
		if (0 != (HalconCpp::HTuple(hv_CurrentColor != HalconCpp::HTuple("")).TupleAnd(hv_CurrentColor != HalconCpp::HTuple("auto"))))
		{
			HalconCpp::SetColor(hv_WindowHandle, hv_CurrentColor);
		}
		else
		{
			HalconCpp::SetRgb(hv_WindowHandle, hv_Red, hv_Green, hv_Blue);
		}
		hv_Row = hv_R1 + (hv_MaxHeight*hv_Index);
		HalconCpp::SetTposition(hv_WindowHandle, hv_Row, hv_C1);
		HalconCpp::WriteString(hv_WindowHandle, HalconCpp::HTuple(hv_String[hv_Index]));
	}
	//reset changed window settings
	HalconCpp::SetRgb(hv_WindowHandle, hv_Red, hv_Green, hv_Blue);
	HalconCpp::SetPart(hv_WindowHandle, hv_Row1Part, hv_Column1Part, hv_Row2Part, hv_Column2Part);
	return;
}

void Locate4::saveResult()
{
	// Local iconic variables
	HalconCpp::HObject  ho_Image, ho_ModelContours, ho_ContoursAffinTrans,
		ho_ContoursTrans, ho_FoundContour;
	HalconCpp::HObject  ho_ObjectSelected, ho_ModelWorld;

	// Local control variables
	HalconCpp::HTuple  hv_CamParam;
	HalconCpp::HTuple  hv_WindowDrawResult;
	HalconCpp::HTuple  hv_HomMat3D, hv_NumberContour;
	HalconCpp::HTuple  hv_K, hv_Y, hv_X, hv_Z, hv_Xc, hv_Yc, hv_Zc, hv_R;
	HalconCpp::HTuple  hv_C;
	HalconCpp::HTuple hv_ModelRow, hv_ModelCol, hv_CamParNoRad, hv_ModelPose;

	hv_CamParam.Clear();
	hv_CamParam[0] = 0.00840228644304546;
	hv_CamParam[1] = -1019.55707294893;
	hv_CamParam[2] = 4.40722435287872e-006;
	hv_CamParam[3] = 4.4e-006;
	hv_CamParam[4] = 828.65339497083;
	hv_CamParam[5] = 633.46017521628;
	hv_CamParam[6] = 1624;
	hv_CamParam[7] = 1234;

	HalconCpp::ReadImage(&ho_Image, "cache.png");
	//获取轮廓投影
	HalconCpp::GetDeformableModelContours(&ho_ModelContours, hv_ModelID, 1);
	HalconCpp::GetDeformableModelParams(hv_ModelID, "model_row", &hv_ModelRow);
	HalconCpp::GetDeformableModelParams(hv_ModelID, "model_col", &hv_ModelCol);
	HalconCpp::GetDeformableModelParams(hv_ModelID, "model_pose", &hv_ModelPose);
	HalconCpp::ChangeRadialDistortionCamPar("preserve_resolution", hv_CamParam, 0, &hv_CamParNoRad);
	HalconCpp::AffineTransContourXld(ho_ModelContours, &ho_ContoursAffinTrans,
		(((HalconCpp::HTuple(1).Append(0))
		.TupleConcat(hv_ModelRow)).
		TupleConcat((HalconCpp::HTuple(0).Append(1))))
		.TupleConcat(hv_ModelCol));
	HalconCpp::ContourToWorldPlaneXld(ho_ContoursAffinTrans, &ho_ContoursTrans,
		hv_CamParNoRad, hv_ModelPose, "m");
	HalconCpp::CountObj(ho_ContoursTrans, &hv_NumberContour);

	HalconCpp::GenEmptyObj(&ho_FoundContour);

	HalconCpp::HTuple end_val40 = hv_NumberContour;
	HalconCpp::HTuple step_val40 = 1;
	for (hv_K = 1; hv_K.Continue(end_val40, step_val40); hv_K += step_val40)
	{
		HalconCpp::SelectObj(ho_ContoursTrans, &ho_ObjectSelected, hv_K);
		HalconCpp::GetContourXld(ho_ObjectSelected, &hv_Y, &hv_X);
		hv_Z = HalconCpp::HTuple(hv_X.TupleLength(), 0.0);
		//Transform the metric model into world coordinates.
		HalconCpp::PoseToHomMat3d(hv_Pose, &hv_HomMat3D);
		HalconCpp::AffineTransPoint3d(hv_HomMat3D, hv_X, hv_Y, hv_Z, &hv_Xc, &hv_Yc, &hv_Zc);
		//Project the 3D points of the model, which are
		//available as world coordinates, into the image
		//coordinate system.
		HalconCpp::Project3dPoint(hv_Xc, hv_Yc, hv_Zc, hv_CamParam, &hv_R, &hv_C);
		HalconCpp::GenContourPolygonXld(&ho_ModelWorld, hv_R, hv_C);
		HalconCpp::ConcatObj(ho_FoundContour, ho_ModelWorld, &ho_FoundContour);
	}
	HalconCpp::OpenWindow(0, 0, 1624, 1234, 0, "invisible",
		"", &hv_WindowDrawResult);
	HalconCpp::DispImage(ho_Image, hv_WindowDrawResult);
	HalconCpp::SetColor(hv_WindowDrawResult, "magenta");
	HalconCpp::SetLineWidth(hv_WindowDrawResult, 5);
	HalconCpp::DispXld(ho_FoundContour, hv_WindowDrawResult);
	HalconCpp::DumpWindow(hv_WindowDrawResult, "png", "ResultSaved4");
	HalconCpp::CloseWindow(hv_WindowDrawResult);
}
